#include <iostream>
#include <chrono>
#include <functional>
#include "geometry_msgs/msg/twist.hpp"
#include "rclcpp/rclcpp.hpp"
#include "turtlesim/msg/pose.hpp"
using namespace std;

class turtle_control : public rclcpp::Node
{
public:
    turtle_control(const string &Node_name, double target_x = 1.0, double target_y = 1.0, double k = 1.0, double max_speed = 3.0) 
    : Node(Node_name)
    ,_target_x(target_x)
    ,_target_y(target_y)
    ,_k(k)
    ,_max_speed(max_speed)
    {
        _velocity_publisher = create_publisher<geometry_msgs::msg::Twist>("/turtle1/cmd_vel", 10);

        _pose_subscription = create_subscription<turtlesim::msg::Pose>("/turtle1/pose", 10,
            bind(&turtle_control::_on_pose_received, this, placeholders::_1));
    }

private:
    void _on_pose_received(const turtlesim::msg::Pose::SharedPtr pose)
    {
        // 发布速度指令
        auto message = geometry_msgs::msg::Twist();
        // 记录当前位置
        double current_x = pose->x;
        double current_y = pose->y;
        RCLCPP_INFO(get_logger(), "当前位置x, y", current_x, current_y);
        // 计算距离
        double distance = sqrt(pow(_target_x - current_x, 2) + pow(_target_y - current_y, 2));
        //计算角度
        double angle = atan2(_target_y - current_y, _target_x - current_x) - pose->theta;

        //计算策略
        if(distance > 0.1)
        {
            if(fabs(angle) > 0.2)
            {
                message.angular.z = fabs(angle);
            }
            else 
            {
                // 通过比例控制器计算输出速度
                message.linear.x = _k * distance;
            }
        }
        //限制最大速度
        // if(message.linear.x > max_speed)
        // {
        //     message.linear.x = _max_speed;
        // }
        message.linear.x = message.linear.x > _max_speed ? message.linear.x : _max_speed;
        _velocity_publisher->publish(message);
    }

private:
    rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr _pose_subscription;
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr _velocity_publisher;
    double _target_x;  // 目标位置
    double _target_y;  // 目标位置
    double _k;         // 比例系数，控制输出=误差*比例系数
    double _max_speed; // 最大速度
};

int main(int argc, char** argv)
{
    rclcpp::init(argc, argv);
    auto node = shared_ptr<turtle_control>(new turtle_control("turtle_controller"));
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}